#ifndef APPLICATIONPROTOTYPE_H_
#define APPLICATIONPROTOTYPE_H_

/*******************************************************************************
ApplicationPrototype.h

This is the module that is the most liquid for development of a new sensor
firmware prototype. In other words, this is where the "work" should be done
by the developer

Origin:
Adapted from parts of VCinterface and AtmelInterface by Travis Young,

Change Log:
2010 May 21, Friday, birth
2010 August 6, Friday, documentation
2011 January 27, Thursday, added pause/resume and user-specified of_scale
******************************************************************************/
#include <avr/eeprom.h>

// enumeration data
#define	ENUM_SW_REV		0x01		// enumeration: software revision (BCD)

// SENSOR COMMANDS: 1 Byte header to define one of 256 different commands
//
// The command headers are defined in different header files, depending on
// the function of the command. In general:
// Commands 0-29: reserved for operating system
// Commands 30-59: reserved for basic vision chip (biases etc.) operation
// Commands 60 and up: reserved for the application and defined here
//
// Below are commands 60 and up for the application.
// some commands are "keyed" which means that the upper bits need to be set in a
// particular way in order for the command to be executed. This helps reduce the
// chance of accidental usage.
#define COMM_CMD_OF_MODE				60		// default 1=regular, 2=special cumulative
#define	COMM_CMD_IMAGE_CAL_ENABLE		61		// KEYED Calibrate! i.e. obtain FPN
#define	COMM_CMD_USE_IMAGE_CAL  		62		// 0 = none, 1 = use
#define COMM_CMD_SROW					63		// not implemented
#define COMM_CMD_SCOL					64		// not implemented
#define COMM_CMD_GET_RAW_IMAGE			65		// 1/0 on whether to acquire raw image
#define COMM_CMD_OF_DEADZONE			66		// modify OF deadzone for computing cumulative OF
#define COMM_CMD_HP_IMAGE				67
#define COMM_CMD_HP_ALPHA				68		// # times to shift for computing HP
#define COMM_CMD_LP_OF					69		// # times to shift for optical flow low-passing
#define COMM_CMD_MIN_DISP				70		// Minimum displacement for cumulative method
#define COMM_CMD_ADDFPN_STRENGTH		71		// strength of added FPN mask
#define COMM_CMD_FRAME_SKIPPING			72		// number of frames between OF calcs
#define COMM_CMD_SETROW_PINHOLE			77		// set pinhole row
#define COMM_CMD_SETCOL_PINHOLE			78		// set pinhole column
#define COMM_CMD_FIND_PINHOLE			79		// KEYED find the pinhole
#define COMM_CMD_GET_FPS                80      // 1/0 on whether to compute frames per second
#define COMM_CMD_LED_MODE               81      // 1 = LED on when OFX>0; 2 = switch state every 256 OF computations
#define COMM_CMD_OF_SCALE				82		// Change OF scaling

#define COMM_CMD_PAUSE					90		// KEYED Stop grabbing new images and calculating flow
#define COMM_CMD_RESUME					91		// Resume normal computation

// DATA SETS: Argument for "COMM_CMD_ATT" command to indicate what data
// set is set for transmit.
//
// The data sets are defined in different header files, depending on
// the function of the command. In general:
// Data sets 0-29: reserved for operating system
// Data set 30-59: reserved for basic vision chip operation
// Data set 60 and up: reserved for the application
//
// Below are data sets 60 and up
#define COMM_ATT_FPS				60		// frames per second
#define	COMM_ATT_DRC				61		// not used
#define	COMM_ATT_RAW				62		// raw 16x16 image
#define	COMM_ATT_CAL				63		// output calibration mask
#define	COMM_ATT_OF_Y				64		// not used
#define COMM_ATT_OFOUT1				65		// six element output
#define COMM_ATT_OF_AVG				66		// not used
#define COMM_ATT_PINHOLE_LOC		67		// pinhole location
#define COMM_ATT_IMAGE_OUT          68		// generic image out- NOTE currently being used to get an 8x8 image
#define COMM_ATT_LAST_CMD			69		// Last command executed
#define COMM_ATT_DEBUG				70		// debug array
#define COMM_ATT_OFREC				71		// optical flow record for debugging
#define COMM_ATT_STATES				72		// all the calibration/state variables that are also saved in EEPROM


/////////////////////////////////////
// FPN mask calibration types
#define	OM_FPN_CAL_NONE		0		// do nothing
#define	OM_FPN_CAL_EXEC		1		// run calibration routine
#define	OM_FPN_CAL_EEPROM	2		// load from EEPROM

//float FPS;

/////////////////////////////////////
struct {
	float FPS;				// new location for FPS variable
	unsigned char locked;	// 1 = locked, 0 = unlocked
	unsigned char read_length;
	unsigned char read[2];
} FPS;

// Note regarding images: Images are generally stored row-wise
// in a 1D array

//size of image array. currently the raw image is just every
// fourth row and column of the full 64x64 array.
#define RAW_IMAGE_SIZE		256	

//image variables
// For the current firmware, we are using an 8x8 image for
// the pinhole image. We can use a larger amount e.g. 16x16
// at most if we want to. Possible change- rename to make 
// it clearly 8x8 or just variable size?.
#define MAX_PINHOLE_IMAGE_SIZE	256  //this is used to reserve space for the arrays
#define PINHOLE_IMAGE_WIDTH 8
#define PINHOLE_IMAGE_HEIGHT 8
#define PINHOLE_IMAGE_SIZE (PINHOLE_IMAGE_WIDTH*PINHOLE_IMAGE_HEIGHT)   
// PINHOLE_IMAGE_WIDTH must be an even number (see get_pinhole_image)
#if ((PINHOLE_IMAGE_WIDTH % 2)!=0)
	#error PINHOLE_IMAGE_WIDTH must be an even number
#endif
// PINHOLE_IMAGE_HEIGHT must be an even number (see get_pinhole_image)
#if ((PINHOLE_IMAGE_HEIGHT % 2)!=0)
	#error PINHOLE_IMAGE_HEIGHT must be an even number
#endif
#define PINHOLE_IMAGE_SIZE (PINHOLE_IMAGE_WIDTH*PINHOLE_IMAGE_HEIGHT)
#if (PINHOLE_IMAGE_SIZE > MAX_PINHOLE_IMAGE_SIZE)
	#error PINHOLE_IMAGE_SIZE must be less than MAX_PINHOLE_IMAGE_SIZE
#endif

struct {
	unsigned char locked;
	unsigned int  read_length;
	unsigned char read[RAW_IMAGE_SIZE];
} raw_image_16x16;

#define get_raw_image state_vars.read[2] // moved here out of the above struct

// Using a single char for 8 flags:
// 0x01 : Pause computation
// 0x02 : currently unused
// 0x04 : currently unused
// 0x08 : currently unused
// 0x10 : currently unused
// 0x20 : currently unused
// 0x40 : currently unused
// 0x80 : currently unused
unsigned char control_flags;

// OFout1 - This is the generic optical flow output vector. 
// the contents depend on the method used to compute optical
// flow.
// OF_mode = 1: First two bytes are X and Y instantaneous OF.
// second two bytes are a simple accumulation.
// third two bytes are a running average of X and Y OF
// OF_mode = 2: Alternative odometry method. First two bytes
// store the accumulation.
struct OFdata {
	unsigned char locked;
	unsigned char read_length;
	unsigned char read[6];
};
struct OFdata OFout0,OFout1;
struct OFdata* pOFout;

// OFrec stores raw optical flow measurements for diagnostic purposes
struct {
	unsigned char locked;
	unsigned char read_length;
	unsigned char read[103];
	unsigned char read_index;
} OFrec;

// ImageBuf variables serve as both working arrays and output arrays
struct ImageBuf{
	unsigned char locked;
	unsigned int  read_length;
	unsigned char read[MAX_PINHOLE_IMAGE_SIZE];
};
struct ImageBuf Image0,Image1,Cal;
//struct ImageBuf ImageOut;
struct ImageBuf *pImageOut;

// Image0 and Image1: Every frame we grab a new image and alternately write to Image0 or Image1.
// Then we set which_Image_later to indicate which of these is the "first" image and which 
// is "second". The optical flow algorithm then uses these two images, in the correct order,
// to compute the current instantaneous optical flow.
unsigned char		which_Image_later;				// which Image is later

unsigned char 		of_count;  // counter for optic flow computations

signed int			ImageLP[MAX_PINHOLE_IMAGE_SIZE];	// low-passed image
unsigned char		ImageLP8[MAX_PINHOLE_IMAGE_SIZE];	// For the alternative high pass function
unsigned char		LP8_skipcnt;					// counter for skipping / lpf
//unsigned char		HPF_shift_amt;					// amount to shift for computing high passed image
#define HPF_shift_amt state_vars.read[4]	// moved here

// skipping frames
//unsigned char		frames_to_skip;		// default is 1			// number of frames to skip- run-time variable
#define frames_to_skip state_vars.read[8]	// moved here
unsigned char		frame_count;					// count of frames.

//unsigned char		use_image_cal;		// flag to use image calibration
#define use_image_cal	state_vars.read[1]	// moved here

// Added FPN mask. This is the strengh of an added FPN mask used for some variations
//unsigned char		AddFPN_strength;			// strength of added FPN mask; run-time variable
#define AddFPN_strength state_vars.read[7]	// moved here

// Optical flow mode
//unsigned char		OF_mode;			// run-time variable
#define OF_mode	state_vars.read[0]	// moved here

// LED mode
//unsigned char		LED_mode;			// run-time variable, defines behavior of LED
#define LED_mode state_vars.read[10]	// moved here

//unsigned char		get_FPS;			// flag to compute frames per second
#define get_FPS state_vars.read[9]	// moved here

// Other optical flow values for cumulative method
unsigned char		frame_startup;		// counts frames since startup to avoid computing OF first few frames
float				OFACCX,OFACCY;		// accumulated optical flow from frames
float				ODOX,ODOY;			// odometry values
//unsigned char		min_disp;			// minimum displacement - run-time variable
#define min_disp state_vars.read[6]	// moved here

////////////////////////////////
// EEPROM VARIABLES
// These are the memory locations of the associated variables.
// The ATmega644P has 2kB EEPROM, so use addresses < 0x0800
// * 0x0000 is used in AVR8sa1os101.h
// For calibration values, each is 1 byte so use the top register to store the
// values, i.e. 0x0700 + the CMD number. They are defined in the respective
// files, this contains application commands.
#define			EEP_pinhole_row		0x0001	// 1 byte
#define			EEP_pinhole_col		0x0002	// 1 byte
#define			EEP_cal_mask		0x0010	// 64 bytes, through 0x50 (PINHOLE_IMAGE_SIZE)

#define			EEP_OF_MODE			( 0x0700 + COMM_CMD_OF_MODE )
#define			EEP_USE_IMAGE_CAL	( 0x0700 + COMM_CMD_USE_IMAGE_CAL )
#define			EEP_GET_RAW_IMAGE	( 0x0700 + COMM_CMD_GET_RAW_IMAGE )
#define			EEP_OF_DEADZONE		( 0x0700 + COMM_CMD_OF_DEADZONE )
#define			EEP_HP_ALPHA		( 0x0700 + COMM_CMD_HP_ALPHA )
#define			EEP_LP_OF			( 0x0700 + COMM_CMD_LP_OF )
#define			EEP_MIN_DISP		( 0x0700 + COMM_CMD_MIN_DISP )
#define			EEP_ADDFPN_STRENGTH	( 0x0700 + COMM_CMD_ADDFPN_STRENGTH )
#define			EEP_FRAME_SKIPPING	( 0x0700 + COMM_CMD_FRAME_SKIPPING )
#define			EEP_GET_FPS			( 0x0700 + COMM_CMD_GET_FPS )
#define			EEP_LED_MODE		( 0x0700 + COMM_CMD_LED_MODE )
#define			EEP_OF_SCALE		( 0x0700 + COMM_CMD_OF_SCALE )

////////////////////////////////
// pinhole location variables
unsigned char		pinhole_row;					// row of brightest pixel
unsigned char		pinhole_col;					// col of brightest pixel
unsigned char		pinhole_val;					// value of brightest pixel
 
struct {
	unsigned char	locked;
	unsigned char	read_length;
	unsigned char	read[3];			// row, col, value
} pinhole_location;


// for these variables: 0) #def header 1) allocate here, 2) write-to if not locked, 3) initialize, 4) mod processxmt
//debug array
struct {
	unsigned char locked;
	unsigned int  read_length;
	unsigned char read[256];
} debug;

// Persistent state variables
struct {
	unsigned char locked;
	unsigned int read_length;
	// read contents:
	// OF_mode, use_image_cal, get_raw_image, OF_deadzone, HPF_shift_amt, rof_shift,
	// min_shift, AddFPN_strength, frames_to_skip, get_FPS, LED_mode, of_scale_byte
	unsigned char read[12];
} state_vars;

void store_cal_mask(void);
void load_cal_mask(void);
void load_pinhole_location(void);
void save_pinhole_location(void);

void Application_Init(void);

void pinhole_find(void);
void get_pinhole_image(unsigned char *const I);
void get_raw_image_16x16(void);

//image functions
void calc_mask(void);

void process_application_command(unsigned char cmd0, unsigned char cmd1);
void process_application_transmit(unsigned char array_to_transmit);
void Application_MainLoop(void);
void process_images(void);

void find_pinhole_fpn_calibration(void);

#endif /*APPLICATIONPROTOTYPE_H_*/
